import time
import datetime
import os
import numpy as np
import av
import cv2
from .ball_tracker import ball_tracker
from .MyTello import MyTello
from cv2 import dnn
from .face_tracker import face_tracker
from .Gesture import Gesture


class TelloCV(object):
    """
    无人机人工智能视觉和控制操作类
    """

    def __init__(self, Tello: MyTello) -> object:

        self.record = False
        self.tracking = True
        self.keydown = False
        self.date_fmt = '%Y-%m-%d_%H%M%S'
        
        self.speed = 30
        """无人机的移动速度，默认值为30"""
        self.speed2 = 20
        """无人机的转向速度，默认值为20"""
        self.drone = Tello
        self.opened_window=False
        
        # self.init_drone()

        self.caffe_prototxt_path = "./MyTelloLib/model/RFB-320.prototxt"
        self.caffe_model_path = "./MyTelloLib/model/RFB-320.caffemodel"
        self.net = dnn.readNetFromCaffe(
            self.caffe_prototxt_path, self.caffe_model_path)

        # container for processing the packets into frames
        # self.container = av.open(self.drone.get_video_stream())
        # self.vid_stream = self.drone.container.streams.video[0]
        self.out_file = None
        self.out_stream = None
        self.out_name = None
        self.start_time = time.time()
        self.upper = None
        self.lower = None
        # tracking a color
        green_lower = (30, 50, 50)
        green_upper = (80, 255, 255)
        #red_lower = (0, 50, 50)
        # red_upper = (20, 255, 255)
        # blue_lower = (110, 50, 50)
        # blue_upper = (130, 255, 255)
        # orange_lower=(10,90,150)
        # orange_upper=(30,255,255)

        self.track_cmd = ""
        self.gesture_frame_count = 0
        # self.ball_tracker = ball_tracker(self.drone.height,
        #                        self.drone.width,
        #                        green_lower, green_upper)
        self.ball_tracker = None
        self.face_tracker = None
        self.hand_tracker = None

        self.prev_flight_data = None
        self.prev_info_data = None
        self.prev_battery_data = None
        self.drone.subscribe(self.drone.EVENT_FLIGHT_DATA,
                             self.flight_data_handler)
        self.drone.subscribe(self.drone.EVENT_BATTERY_DATA,
                             self.flight_battery_handler)
        self.drone.subscribe(self.drone.EVENT_INFO_DATA,
                             self.flight_info_handler)

    def init_ball_tracker(self, lower, upper):
        """初始化小球追踪器

        Args:
            lower (H,S,V): 识别小球的hsv颜色最小值
            upper (H,S,V): 识别小球的hsv颜色最大值
        """
        self.ball_tracker = ball_tracker(self.drone.height,
                                         self.drone.width,
                                         lower, upper)

    def init_face_tracker(self):
        """初始化人脸追踪器"""
        self.face_tracker = face_tracker(self.drone.height,
                                         self.drone.width,)

    def init_hand_tracker(self):
        """初始化手部追踪器"""
        self.hand_tracker = Gesture()

    # def init_drone(self):
    #     """Connect, uneable streaming and subscribe to events"""
    #     # self.drone.log.set_level(2)
    #     # self.drone.connect()
    #     # self.drone.start_video()

    def __del__(self):
        try:
            self.drone.unsub(self.drone.EVENT_FLIGHT_DATA)
            self.drone.unsub(self.drone.EVENT_INFO_DATA)
            self.drone.unsub(self.drone.EVENT_BATTERY_DATA)
        finally:
            pass

    def hand_detect(self, frame):
        """识别画面的手势。可以识别出： '', '0', '1', '2', '3', '4', '5', '6', '8', 'Pink,Up', 'Thumb,Up', 'Princess', 'Bye', 'Spider-Man', 'Rock,n,Roll', 'Unknown'

        Args:
            frame (list, optional): 取得的无人机视频的一帧画面. Defaults to [].

        Returns:
            hand_jugg(str): 识别出的左手或右手
            gesture_str(str):识别出的手势名称
            image(list):标注手势后的视频一帧图像
        """
        hand_jugg, Gesture_str, image = self.hand_tracker.detect(frame)
        image = self.write_hud(image)
        return hand_jugg, Gesture_str, image

    def face_position(self, frame):
        """查找并标注人脸的位置

        Args:
            frame : 从飞机取得的视频的一帧画面

        Returns:
            xoff:画面中心为原点，人脸的横坐标
            yoff:画面中心为原点，人脸的纵坐标
            zoff:飞机前50厘米为原点，人脸的深度坐标
            image:添加人脸位置和方向标注后的视频的一帧画面

        """

        image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
        xoff, yoff, zoff = self.face_tracker.track(image, self.net)
        image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        image = self.face_tracker.draw_arrows(image)
        image = self.write_hud(image)
        return xoff, yoff, zoff, image

    def ball_position(self, frame):
        """查找并标注小球的位置

        Args:
            frame : 从飞机取得的视频的一帧画面

        Returns:
            xoff:画面中心为原点，小球的横坐标\n
            yoff:画面中心为原点，小球的纵坐标\n
            zoff:飞机前50厘米为原点，小球的深度坐标\n
            image:添加小球位置和方向标注后的视频的一帧画面\n

        """

        xoff, yoff, zoff = self.ball_tracker.track(frame)
        image = self.ball_tracker.draw_arrows(frame)
        image = self.write_hud(image)
        return xoff, yoff, zoff, image

    def cmd_control(self, hand_jugg, gesture_str,skip=50):
        """手势名称控制无人机\n
        起飞：takeoff ->'Thumb,up'\n
        降落：land ->'pink,up'\n
        左移：left ->'Left' and '8'\n
        右移：right ->'right' and '8'\n
        前进：forward ->'Spider-Man'\n
        后退：backward ->'Rock,n,Roll'\n
        左转：rotate cc=-1 -> 'Left' and '4'\n
        右转：rotate cc=1 ->'right' and '4'\n
        上升：up ->'1'\n
        下降：down ->'0'\n
        停止：stop->'5','unknow',''\n
        
        Args:
            hand_jugg (_type_): 左手或右手('Left','Right')\n
            gesture_str (_type_): 手势名称:'', '0', '1', '2', '3', '4', '5', '6', '8', 'Pink,Up', 'Thumb,Up', 'Princess', 'Bye', 'Spider-Man', 'Rock,n,Roll', 'Unknown'\n
            skip(int):跳过多少帧识别一次
            
        """
        self.gesture_frame_count +=1
        cc = 1
        cmd=''
        if gesture_str == 'Thumb,Up':
            cmd = 'takeoff'
        if gesture_str == 'Pink,Up':
            cmd = 'land'
        if hand_jugg == 'Left' and gesture_str == '8':
            cmd = 'left'
        if hand_jugg == 'Right' and gesture_str == '8':
            cmd = 'right'
        if gesture_str == '0':
            cmd = 'down'
        if gesture_str == '1':
            cmd = 'up'
        if hand_jugg == 'Left' and gesture_str == '4':
            cmd = 'rotate'
            cc = -1
        if hand_jugg == 'Right' and gesture_str == '4':
            cmd = 'rotate'
        if gesture_str == 'Spider-Man':
            cmd = 'forward'
        if gesture_str == 'Rock,n,Roll':
            cmd = 'backward'
        if gesture_str == '5':
            cmd = 'stop'


        if self.gesture_frame_count>=skip and cmd != '':
            self.gesture_frame_count=0
            print("当前的命令：", cmd)
            if cmd == 'forward' or cmd == 'backward':
                getattr(self.drone.flight, cmd)(self.speed2)
            if cmd == 'takeoff' or cmd == 'land' or cmd == 'stop':
                getattr(self.drone.flight, cmd)()
            else:
                getattr(self.drone.flight, cmd)(self.speed*cc)

    def xyz_control(self, xoff, yoff, zoff, range=15, distance=50):
        """用坐标控制无人机

        Args:
            xoff (int): 画面中心为原点，小球的横坐标
            yoff (int): 画面中心为原点，小球的纵坐标
            zoff (int): 飞机前50厘米为原点，小球的深度坐标
            range (int): 指定不控制飞机的深度坐标范围。默认值：15
            distance (int): 指定不控制飞机的横、纵坐标的范围。默认值：50

        Returns:
            None :无返回值
        """

        cmd = ""
        if self.tracking:
            if xoff < -distance:
                cmd = "counter_clockwise"
            elif xoff > distance:
                cmd = "clockwise"
            elif yoff < -distance:
                cmd = "down"
            elif yoff > distance:
                cmd = "up"
            elif zoff < -range:
                cmd = "backward"
            elif zoff > range:
                cmd = "forward"
            else:
                if self.track_cmd != "":
                    getattr(self.drone, self.track_cmd)(0)
                    self.track_cmd = ""

        if cmd is not self.track_cmd:
            if cmd != "":
                if cmd == "forward" or cmd == "backward":
                    print("track command forward:", cmd)
                    getattr(self.drone, cmd)(self.speed2)
                    self.track_cmd = cmd
                else:
                    print("track command other:", cmd)
                    getattr(self.drone, cmd)(self.speed)
                    self.track_cmd = cmd
        return

    # def ball_process_frame(self, frame):
    #     """convert frame to cv2 image and show"""
    #     # image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
    #     image = frame

    #     # if self.record:
    #     #     self.record_vid(frame)

    #     xoff, yoff, zoff = self.ball_tracker.track(image)
    #     image = self.ball_tracker.draw_arrows(image)
    #     image = self.write_hud(image)
    #     distance = 50
    #     cmd = ""
    #     if self.tracking:
    #         if xoff < -distance:
    #             cmd = "counter_clockwise"
    #         elif xoff > distance:
    #             cmd = "clockwise"
    #         elif yoff < -distance:
    #             cmd = "down"
    #         elif yoff > distance:
    #             cmd = "up"
    #         elif zoff < -15:
    #             cmd = "backward"
    #         elif zoff > 15:
    #             cmd = "forward"
    #         else:
    #             if self.track_cmd != "":
    #                 getattr(self.drone, self.track_cmd)(0)
    #                 self.track_cmd = ""

    #     if cmd is not self.track_cmd:
    #         if cmd != "":
    #             if cmd == "forward" or cmd == "backward":
    #                 print("track command forward:", cmd)
    #                 getattr(self.drone, cmd)(self.speed)
    #                 self.track_cmd = cmd
    #             else:
    #                 print("track command other:", cmd)
    #                 getattr(self.drone, cmd)(self.speed)
    #                 self.track_cmd = cmd
    #     return image

    # def face_process_frame(self, frame):
    #     """convert frame to cv2 image and show"""
    #     # if frame is None:
    #     #     return
    #     image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)

    #     # cv2.imshow("test",image)
    #     # cv2.waitKey(1)
    #     if self.record:
    #         self.record_vid(frame)

    #     xoff, yoff, zoff = self.face_tracker.track(image, self.net)
    #     # image = self.tracker_face.draw_arrows(image)
    #     # print(zoff)
    #     image = self.write_hud(image)
    #     distance = 80
    #     cmd = ""
    #     # if count!=0:
    #     #     if self.track_cmd is not "":
    #     #         getattr(self.drone, self.track_cmd)(0)
    #     #         self.track_cmd = ""
    #     #     return image
    #     if self.tracking:
    #         if xoff < -distance:
    #             cmd = "counter_clockwise"
    #         elif xoff > distance:
    #             cmd = "clockwise"
    #         elif yoff < -distance:
    #             cmd = "down"
    #         elif yoff > distance:
    #             cmd = "up"
    #         elif zoff < -15:
    #             cmd = "backward"
    #             # self.backtimes = self.backtimes + 1
    #             # print("backtimes={}".format(self.backtimes))
    #         elif zoff > 15:
    #             cmd = "forward"
    #             # self.fortimes = self.fortimes + 1
    #             # print("fortimes={}".format(self.fortimes))
    #         else:
    #             if self.track_cmd != "":
    #                 getattr(self.drone, self.track_cmd)(0)
    #                 self.track_cmd = ""

    #     if cmd is not self.track_cmd:
    #         if cmd != "":
    #             if cmd == "forward" or cmd == "backward":
    #                 print("track command forward:", cmd)
    #                 getattr(self.drone, cmd)(self.speed2)
    #                 self.track_cmd = cmd
    #             else:
    #                 print("track command other:", cmd)
    #                 getattr(self.drone, cmd)(self.speed)
    #                 self.track_cmd = cmd

    #     return image

    def write_hud(self, frame):
        """在图像上显示"""
        if self.prev_flight_data == None:
            return frame
        stats = self.prev_flight_data.split('|')
        stats.extend( self.prev_info_data.split('|'))
        stats.append(self.prev_battery_data)
        stats.append("Tracking:" + str(self.tracking))
        for idx, stat in enumerate(stats):
            text = stat.lstrip()
            cv2.putText(frame, text, (0, 30 + (idx * 30)),
                        cv2.FONT_HERSHEY_SIMPLEX,
                        0.5, (0, 255, 255), lineType=30)
        return frame

    def flight_data_handler(self,  data):
        """Listener to flight data from the drone."""
        yaw, pitch, roll = data
        text = "Drone attitude:| yaw:{0},| pitch:{1},| roll:{2} ".format(
            yaw, pitch, roll)
        # print (text)
        if self.prev_flight_data != text:
            self.prev_flight_data = text
    def flight_info_handler(self,  data):
        """Listener to flight data from the drone."""
        alt, temp, motor_time = data
        text = "alt:{0},| temp:{1},| time:{2} ".format(
            alt, temp, motor_time)
        # print (text)
        if self.prev_info_data != text:
            self.prev_info_data = text
            
    def flight_battery_handler(self,  data):
        """Listener to flight data from the drone."""
        
        text = "battery:{0}".format(
            data)
        # print (text)
        if self.prev_battery_data != text:
            self.prev_battery_data = text

    def show(self,frame):
        """显示图像并接收ESC键

        Args:
            frame (_type_): 图像

        Returns:
            _type_: 按了ESC返回True
        """
        cv2.imshow("result", frame)
        self.opened_window=True
        if cv2.waitKey(1) & 0xff == 27:
            return True
        return False
    
    def desctory_show(self):
        cv2.destroyAllWindows()
        
    def quit(self):
        if self.opened_window:
            cv2.destroyAllWindows()
